//
// Created by okgwf on 2021/9/16.
//

#include "SixAxisImuPose.h"
/*
陀螺仪: gyroscope   /ˈdʒaɪrəskoʊp/
重力:  gravity  /ˈɡrævəti/
加速度计: accelerometer:  /əkˌseləˈrɑːmɪtər/
磁力计: magnetometer /ˌmæɡnəˈtɑːmɪtər; ˌmæɡnəˈtɑːmətər /
 */

#define sampleFreq	100.f			// sample frequency in Hz

//这里的Kp,Ki是用于控制加速度计修正陀螺仪积分姿态的速度
#define Kp 10.0f  //2.0f
#define Ki 0.008f  //0.002f
/*采样周期的一半，用于求解四元数微分方程时计算角增量
请确定自己的姿态结算周期: 10ms*/
#define halfT 0.005f

//初始姿态四元数(世界坐标系)，变换四元数公式得来: w=1  q(1,0,0,0)
static float q0 = 1, q1 = 0, q2 = 0, q3 = 0; //最优估计四元数
//定义姿态解算误差的积分.      积分: (integral)           另微分: differential    误差:error
//当前加计测得的重力加速度在三轴(x,y,z)上的分量,与用当前姿态计算得来的重力在三轴上的分量的 误差的积分
static float xErrorInt = 0, yErrorInt = 0, zErrorInt = 0;

/*
 * 6轴陀螺仪姿态融合算法:  Mahony的互补滤波算法 6轴版
 * 单位: m/s^2   rad/s
 * 由于加速度的噪音很大, 此处建议使用滤波后的数据
 * */
void SixAxisImuUpdate(float gx, float gy, float gz, float ax, float ay, float az, IMU_Angle* angle)//g表陀螺仪，a表加计
{
    float q0temp,q1temp,q2temp,q3temp;//四元数暂存变量，求解"微分方程"时要用
    float norm; //矢量的模或四元数的范数
    float posture_x, posture_y, posture_z;//当前姿态计算得来的重力在三轴上的分量      最优估计姿态:posture
    float error_x, error_y, error_z;//当前加计测得的重力加速度在三轴上的分量,与用当前姿态计算得来的重力在三轴上的分量的误差

    // 先把这些用得到的值算好
    float q0q0 = q0*q0, q0q1 = q0*q1, q0q2 = q0*q2, q0q3 = q0*q3, q1q1 = q1*q1, q1q2 = q1*q2;
    float q1q3 = q1*q3, q2q2 = q2*q2, q2q3 = q2*q3, q3q3 = q3*q3;

    //if(ax*ay*az==0)//加计处于自由落体状态时不进行姿态解算，因为会产生分母无穷大的情况
    if( (ax == 0.0f) && (ay == 0.0f) && (az == 0.0f) )//加计处于自由落体状态时不进行姿态解算，因为会产生分母无穷大的情况
       return;

    //将加速度的原始数据进行归一化,得到单位加速度
    norm = sqrt(ax*ax + ay*ay + az*az);//单位化加速度计，
    ax = ax / norm;// 这样变更了量程也不需要修改KP参数，因为这里归一化了
    ay = ay / norm;
    az = az / norm;

    //用当前姿态(参考坐标n系)计算出重力在三个轴上的分量(载体坐标b系)， Estimated direction of gravity and vector perpendicular to magnetic flux
    //参考坐标n系转化到载体坐标b系的用四元数表示的方向余弦矩阵第三列即是
    /*把四元数换算成"方向余弦矩阵"中的第三列的三个元素.根据余弦矩阵和欧拉角的定义,地理坐标系的重力向量,转到载体坐标系,正好是这三个元素.
     * posture_x,posture_y,posture_z其实就是当前的载体坐标参照系上,换算出来的重力单位向量.(用表示载体姿态的四元数进行换算)*/
    posture_x = 2*(q1q3 - q0q2);
    posture_y = 2*(q0q1 + q2q3);
    posture_z = q0q0 - q1q1 - q2q2 + q3q3;

    /*
     * 在载体坐标参照系上,加速度计测出来的重力向量是ax,ay,az;陀螺仪积分后的姿态推算(估计)出的最优重力方向向量是posture_x,posture_y,
     * posture_z; 它们之间的误差向量,就是陀螺仪积分后的姿态和加速度计测出来的姿态之间的误差.
     * 向量间的误差,可以用向量积(也叫外积,叉积)来表示,error_x,error_y,error_z就是两个重力方向的叉积(法向量).这个叉积向量(法向量)
     * 依旧是位于载体坐标系上的,而陀螺仪积分误差也是在载体坐标系,而且叉积的大小与陀螺积分误差成正比,正好拿来纠正陀螺仪.又由于陀螺仪是对
     * 载体直接积分,所以对陀螺的纠正量会直接体现在对载体坐标系的纠正上.
     * */
    /*
     * 这个误差指的是两个向量的不平行程度，和夹角的正弦成正比，叉乘=|a|*|b|*sin(角度),两个向量又归一化了，所以就和sin(角度成正比)，
     * 进而反应了陀螺仪估计的姿态和加速度计测量的姿态误差.
     * Mahony那篇论文用叉乘来表示误差是有其物理意义的。一般来说向量的误差不是用叉乘来表示，直接作减法会更合适。而单位向量的向量叉积在物理意义上
     * 在小角度意义就是轴角，轴角本身就刻画了旋转，也就是叉乘的结果可以看成是旋转误差，在这个物理意义下比任何误差表示都要合理和切合实际系统
     * */
    //计算: 传感器测得的重力与当前姿态计算的重力间的误差，向量外积可以表示这一误差
    //原因我理解是因为两个向量是单位向量且sin0等于0不过要是夹角是180度呢~这个还没理解
    //a X b = |a|*|b|*sinθ
    // Error is sum of cross product between estimated and measured direction of gravity
    error_x = (ay*posture_z - az*posture_y) ;
    error_y = (az*posture_x - ax*posture_z) ;
    error_z = (ax*posture_y - ay*posture_x) ;

    //对两种重力分量的叉积误差进行积分
    xErrorInt = xErrorInt + error_x * Ki;// * (1.0f / sampleFreq);
    yErrorInt = yErrorInt + error_y * Ki;// * (1.0f / sampleFreq);
    zErrorInt = zErrorInt + error_z * Ki;// * (1.0f / sampleFreq);

    // adjusted gyroscope measurements
    //对两种重力分量的叉积误差的积分做p,i修正陀螺零偏,通过调节Kp,Ki两个参数,可以控制加速度计修正陀螺仪积分姿态的速度
    gx = gx + Kp*error_x + xErrorInt;  //将误差PI后补偿到陀螺仪，即补偿零点漂移
    gy = gy + Kp*error_y + yErrorInt;
    gz = gz + Kp*error_z + zErrorInt; //这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减

    // Integrate rate of change of quaternion
    //gx *= (1.0f / sampleFreq);		// pre-multiply common factors
    //gy *= (1.0f / sampleFreq);
    //gz *= (1.0f / sampleFreq);

    //下面进行姿态的更新，也就是四元数微分方程的求解
    q0temp=q0;//暂存当前值用于计算
    q1temp=q1;//网上传的这份算法大多没有注意这个问题，在此补上
    q2temp=q2;
    q3temp=q3;

    //四元数微分方程. 采用一阶毕卡解法，融合当前位姿和陀螺仪测量值,并转换到世界参考坐标系N. 相关知识可参见《惯性器件与惯性导航系统》P212
    q0 = q0temp + (-q1temp*gx - q2temp*gy -q3temp*gz)*halfT;
    q1 = q1temp + (q0temp*gx + q2temp*gz -q3temp*gy)*halfT;
    q2 = q2temp + (q0temp*gy - q1temp*gz +q3temp*gx)*halfT;
    q3 = q3temp + (q0temp*gz + q1temp*gy -q2temp*gx)*halfT;

    //对当前姿态四元数进行单位化,单位化四元数在空间旋转时不会拉伸，仅有旋转角度，这类似线性代数里的正交变换
    norm = sqrt(q0q0 + q1q1 + q2q2 + q3q3);
    q0 = q0 / norm;
    q1 = q1 / norm;
    q2 = q2 / norm;
    q3 = q3 / norm;

    //再次把这些用得到的值算好
    q0q0 = q0q0; q0q1 = q0*q1; q0q2 = q0*q2; q0q3 = q0*q3; q1q1 = q1*q1; q1q2 = q1*q2;
    q1q3 = q1*q3; q2q2 = q2*q2; q2q3 = q2*q3; q3q3 = q3*q3;

    //飞行器基于x轴为纵轴: 四元数到欧拉角的转换，
    /*angle->roll  = atan2f(2.f * (q0q1 + q2q3),1 - 2.f * ( q1q1 - q2q2) ); // roll: X轴
    angle->pitch  = asinf(2.f * (q0q2 - q1q3) ); // pitch: Y轴
    //其中YAW航向角由于加速度计对其没有修正作用，因此此处直接用陀螺仪积分代替
    angle->yaw   = atan2f(2.f * (q0q3 + q1q2),1 - 2.f * (q2q2 + q3q3) ); // yaw: Z轴
    */

    //陆地四轮车基于y轴为纵轴: 四元数到欧拉角的转换，
    //不符合翻滚角,左高右低为正
    //angle->roll  = atan2f(-2.f * (q0q3 + q0q2),q0q0 - q1q1 - q2q2 + q3q3 ); // roll: Y轴
    //使符合翻滚角,左高右低为正  (去了一个负号)
    angle->roll  = atan2f(2.f * (q0q3 + q0q2),q0q0 - q1q1 - q2q2 + q3q3 ); // roll: Y轴
    angle->pitch  = asinf(2.f * (q2q3 + q0q1) ); // pitch: x轴
    //其中YAW航向角由于加速度计对其没有修正作用，因此此处直接用陀螺仪积分代替
    angle->yaw   = 2*atan2f(2.f * (q1q2 - q0q3),q0q0 - q1q1 + q2q2 - q3q3 ); // yaw: Z轴
}
